#ifndef MYROBOT_H
#define MYROBOT_H

#include "WPILib.h"
#include "GearToothPID.h"
#include "list"
#include "InsightLT/InsightLT.h"
#include "sockLib.h"
#include "netdb.h"
#include "pthread.h"
#include "networktables2/util/system.h"

using namespace insight;

//Networking
#define ONBOARD_IP "10.8.34."
#define SERVERPORT "8341"
#define LISTENPORT "8340"
#define RCVBUFFER 100
#define SLEEP_CYCLES 200
#define FEED_SHOOT "TT"
#define FEED_PICKUP "PP"

//Targeting Data
#define TX_NOTARGET	0x01
#define TX_LEFT		0x02
#define TX_RIGHT	0x04
#define TX_ONTARGET	0x08
#define TY_NOTARGET	0x10
#define TY_FORWARD	0x20
#define TY_BACKWARD	0x40
#define TY_ONTARGET	0x80

//PID
#define RPMP 				.001
#define RPMI 				0 //.00005
#define RPMD 				0
#define PIDSET 				4500

//Gear Shifting
#define LOWGEARSERVOPOS 		0.0
#define HIGHGEARSERVOPOS 		1.0

//Indexer
#define INDEXERWAITPERIOD		0.0
#define INDEXERSPEED			-0.25

//xbox Digital Controls
#define XBOX_A 			1
#define XBOX_B 			2
#define XBOX_X 			3
#define XBOX_Y 			4
#define XBOX_LBUMP 		5
#define XBOX_RBUMP 		6
#define XBOX_BACK 		7
#define XBOX_START 		8
#define XBOX_LSTICK 	9
#define XBOX_RSTICK 	10

//xbox Analog Controls
#define XBOX_LSTICK_X 	1
#define XBOX_LSTICK_Y 	2
#define XBOX_TRIGGERS	3	//-1 = right; +1 = left
#define XBOX_RSTICK_X 	4
#define XBOX_RSTICK_Y 	5

#define JOYSTICKDEADSPACE 		0.05

/** 
 * This file is included by all other files that communicate with the robot
 * and each other. All components (physically) on the robot need to be
 * declared in this file, and initialized by the constructor in MyRobot.cpp.
 * Any methods that are added later MUST be defined in this file, or other
 * files won't be able to call those methods. All methods (ideally) should
 * be in their own file, to make everything easier to find, read, debug, etc.,
 * but groups of methods performing a common task is also a good idea.
 */

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */ 
class RobotDemo : public SimpleRobot
{
	//Anything on the robot physically that the program needs to
	//communicate with MUST be defined here before using!
	Victor driveFLeft;
	Victor driveBLeft;
	Victor driveFRight;
	Victor driveBRight;
	RobotDrive myRobot; 		// robot drive system
	Servo driveShiftLeft;		// used to change gears on the transmissions
	Servo driveShiftRight;
	Servo loader;				// used to index frisbees from feeding (the top)
	
	Joystick leftStick;			// left joystick
	Joystick rightStick; 		// right joystick333
	Joystick xbox;				// xbox controller
	
	InsightLT display;			// the InsightLT display
	DecimalData dispBat;		// battery voltage display
	StringData dispAimX;		// vision tracking display
	StringData dispAimY;
	
	//Sockets
	int sockfd, rcvsockfd;
	struct addrinfo hints, *servinfo, *p;
	struct addrinfo rcvhints, *rcvservinfo, *rcvp;
	struct sockaddr_storage their_addr;
	unsigned char rcvData;
	bool displayShootFeed;
	std::string targetX;	//the text to display on smart dashboard and Insight
	std::string targetY;	//based on targeting information (instead of '0x11' etc.)
	
	//Motor Controllers
	Victor pickUp;				// motor to pick up frisbees off the floor
	Victor indexer; 			// motor to spin the indexer (pushes into shooter)
	Victor shooterFirst;		// the first wheel the frisbee hits when shot
	Victor shooterSecond;		// the second wheel the frisbee hits when shot
	Victor pyramidLifter;		// the motor to lift the robot onto the bottom rung
	
	//Digital Inputs (sensors)
	DigitalInput pyramidIn;		// arm for lifting robot is fully retracted
	DigitalInput pyramidOut; 	// arm for lifting robot is fully extended
	DigitalInput indexerSensor;	// photosensor to detect the indexer
	
	//RPM and PID control stuff
	DigitalInput encoderFirst;
	DigitalInput encoderSecond;
	GearToothPID RPMGearToothFirst;
	GearToothPID RPMGearToothSecond;
	PIDController PIDfirst;
	PIDController PIDsecond;

public:
	RobotDemo();
	
	void Autonomous();		// *Autonomous-running code
	void Disabled();		// *Robot Disabled code
	void OperatorControl(); // *Code for the rest of the match
	void Test();			// *Method to test code (not for actual competition)
	
	void UpdateSmartDashboard(bool testing = false);	// Method to display info to the Smart Dashboard
	void InitUDP();
	static void* SendUDP(void*);
	static void* RcvUDP(void*);
};

#endif
